﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Microsoft.Robotics.PhysicalModel;
using Microsoft.Robotics.Simulation.Engine;
using System.Drawing.Drawing2D;
namespace SimJaguarsControl
{
    public partial class MainForm : Form
    {
        FromWinformEvents _fromWinformPort;
        private const double ratio = 50;
        private const int START_X = 0;
        private const int START_Y = 120;
        private const int WIDTH_SUB = 40;
        bool sdisplayLaserAux = false; // for displaying only one time the simulation laser background.= top view.
        public bool manualcontrol = false;
        public bool releasecontrol = false;
        public bool manualcontrol2 = false;
        public bool releasecontrol2 = false;
        public bool manualcontrol3 = false;
        public bool releasecontrol3 = false;

        public MainForm(FromWinformEvents EventsPort)
        {
            _fromWinformPort = EventsPort;
            InitializeComponent();
            ManualControls1.Enabled = false;
            ManualControls2.Enabled = false;
            ManualControls3.Enabled = false;

            _cameraImage.MouseMove += new MouseEventHandler(_cameraImage_MouseMove);
            _cameraImage.MouseDown += new MouseEventHandler(_cameraImage_MouseDown);
            _cameraImage2.MouseMove += new MouseEventHandler(_cameraImage_MouseMove);
            _cameraImage2.MouseDown += new MouseEventHandler(_cameraImage_MouseDown);
            _cameraImage3.MouseMove += new MouseEventHandler(_cameraImage_MouseMove);
            _cameraImage3.MouseDown += new MouseEventHandler(_cameraImage_MouseDown);
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Loaded, null, this));
        }

        public void MainForm_Load(object sender, EventArgs e)
        {
            SimulatorConfiguration config = new SimulatorConfiguration(true);
            config.Headless = true;
            SimulationEngine.GlobalInstancePort.Update(config);
        }

        private void MainForm_FormClosed(object sender, EventArgs e)
        {
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Close, null, null));
        }

        #region Embedded Simulator Functions
        Vector2 _mouseDown;
        void _cameraImage_MouseDown(object sender, MouseEventArgs e)
        {
            _mouseDown = new Vector2(e.X, e.Y);
        }

        void _cameraImage_MouseMove(object sender, MouseEventArgs e)
        {
            if (e.Button == MouseButtons.Left)
            {
                _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Drag, null, new Vector2(e.X - _mouseDown.X, e.Y - _mouseDown.Y)));
                _mouseDown = new Vector2(e.X, e.Y);
            }
            else if (e.Button == MouseButtons.Right)
            {
                _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Zoom, null, new Vector2(e.X - _mouseDown.X, e.Y - _mouseDown.Y)));
                _mouseDown = new Vector2(e.X, e.Y);
            }
        }
        public void SetHeadless(bool headless)
        {
            checkBox1.Checked = headless;
            this.Refresh();
        }

        public void SetCameraImage(Bitmap bmp)
        {
            _cameraImage.Image = bmp;
            _cameraImage.Invalidate();
            _cameraImage2.Image = bmp;
            _cameraImage2.Invalidate();
            _cameraImage3.Image = bmp;
            _cameraImage3.Invalidate();
        }

        private void checkBox1_CheckedChanged(object sender, EventArgs e)
        {
            SimulatorConfiguration config = new SimulatorConfiguration(true);
            config.Headless = checkBox1.Checked;
            SimulationEngine.GlobalInstancePort.Update(config);
        }
        #endregion

        #region Drawing
        public Image RotateImage(Image inputImg, double degreeAngle)
        {

            //Corners of the image
            PointF[] rotationPoints = { new PointF(0, 0),
                                        new PointF(inputImg.Width, 0),
                                        new PointF(0, inputImg.Height),
                                        new PointF(inputImg.Width, inputImg.Height)};

            //Rotate the corners
            PointMath.RotatePoints(rotationPoints, new PointF(inputImg.Width / 2.0f, inputImg.Height / 2.0f), degreeAngle);

            //Get the new bounds given from the rotation of the corners
            //(avoid clipping of the image)
            Rectangle bounds = PointMath.GetBounds(rotationPoints);

            //An empy bitmap to draw the rotated image
            Bitmap rotatedBitmap = new Bitmap(bounds.Width, bounds.Height);

            using (Graphics g = Graphics.FromImage(rotatedBitmap))
            {
                g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
                g.InterpolationMode = InterpolationMode.HighQualityBicubic;

                //Transformation matrix
                System.Drawing.Drawing2D.Matrix m = new System.Drawing.Drawing2D.Matrix();
                m.RotateAt((float)degreeAngle, new PointF(inputImg.Width / 2.0f, inputImg.Height / 2.0f));
                m.Translate(-bounds.Left, -bounds.Top, MatrixOrder.Append); //shift to compensate for the rotation

                g.Transform = m;
                g.DrawImage(inputImg, 0, 0);
            }
            return (Image)rotatedBitmap;
        }
        public static class PointMath
        {
            private static double DegreeToRadian(double angle)
            {
                return Math.PI * angle / 180.0;
            }

            public static PointF RotatePoint(PointF pnt, double degreeAngle)
            {
                return RotatePoint(pnt, new PointF(0, 0), degreeAngle);
            }

            public static PointF RotatePoint(PointF pnt, PointF origin, double degreeAngle)
            {
                double radAngle = DegreeToRadian(degreeAngle);

                PointF newPoint = new PointF();

                double deltaX = pnt.X - origin.X;
                double deltaY = pnt.Y - origin.Y;

                newPoint.X = (float)(origin.X + (Math.Cos(radAngle) * deltaX - Math.Sin(radAngle) * deltaY));
                newPoint.Y = (float)(origin.Y + (Math.Sin(radAngle) * deltaX + Math.Cos(radAngle) * deltaY));

                return newPoint;
            }

            public static void RotatePoints(PointF[] pnts, double degreeAngle)
            {
                for (int i = 0; i < pnts.Length; i++)
                {
                    pnts[i] = RotatePoint(pnts[i], degreeAngle);
                }
            }

            public static void RotatePoints(PointF[] pnts, PointF origin, double degreeAngle)
            {
                for (int i = 0; i < pnts.Length; i++)
                {
                    pnts[i] = RotatePoint(pnts[i], origin, degreeAngle);
                }
            }

            public static Rectangle GetBounds(PointF[] pnts)
            {
                RectangleF boundsF = GetBoundsF(pnts);
                return new Rectangle((int)Math.Round(boundsF.Left),
                                     (int)Math.Round(boundsF.Top),
                                     (int)Math.Round(boundsF.Width),
                                     (int)Math.Round(boundsF.Height));
            }

            public static RectangleF GetBoundsF(PointF[] pnts)
            {
                float left = pnts[0].X;
                float right = pnts[0].X;
                float top = pnts[0].Y;
                float bottom = pnts[0].Y;

                for (int i = 1; i < pnts.Length; i++)
                {
                    if (pnts[i].X < left)
                        left = pnts[i].X;
                    else if (pnts[i].X > right)
                        right = pnts[i].X;

                    if (pnts[i].Y < top)
                        top = pnts[i].Y;
                    else if (pnts[i].Y > bottom)
                        bottom = pnts[i].Y;
                }

                return new RectangleF(left,
                                      top,
                                     (float)Math.Abs(right - left),
                                     (float)Math.Abs(bottom - top));
            }
        }

        private Color LinearColor(Color nearColor, Color farColor, int nearLimit, int farLimit, int value)
        {
            if (value <= nearLimit)
            {
                return nearColor;
            }
            else if (value >= farLimit)
            {
                return farColor;
            }

            int span = farLimit - nearLimit;
            int pos = value - nearLimit;

            int r = (nearColor.R * (span - pos) + farColor.R * pos) / span;
            int g = (nearColor.G * (span - pos) + farColor.G * pos) / span;
            int b = (nearColor.B * (span - pos) + farColor.B * pos) / span;

            return Color.FromArgb(r, g, b);
        }

        public void sDrawLaser(int[] data, int robotID)
        {
            if (!sdisplayLaserAux)
            {
                sdisplayLaserAux = true;
                sdrawLaserBackground();
            }
            sDrawSensor(data, robotID);
        }
        private void sdrawLaserBackground()
        {
            Bitmap bmp = new Bitmap(sLaserTopView.Width, sLaserTopView.Height);
            Graphics g = Graphics.FromImage(bmp);

            int width = bmp.Width - 50;
            int height = bmp.Height;

            g.Clear(Color.Lavender);
            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;

            int centerX = bmp.Width / 2;
            int centerY = bmp.Height - 35;
            //g.DrawLine(Pens.Blue, 0, bmp.Height- 120, bmp .Width , bmp.Height -120 );
            //g.DrawLine(Pens.Blue, bmp.Width /2, 0, bmp.Width/2 , bmp.Height- 120);

            for (int i = 0; i < 6; ++i)
            {
                Rectangle RECT = new Rectangle(centerX - 25 * (i + 1), centerY - 25 * (i + 1), 25 * (i + 1) * 2, 25 * (i + 1) * 2);

                g.DrawArc(Pens.Black, RECT, 180, 180);

                //g.DrawArc(Pens.Green, centerX - 25 * (i + 1), centerY - 25 * (i + 1), 25 * (i + 1) * 2, 25 * (i + 1) * 2, 150, 240);
            }

            Font drawFont = new Font("Arial", 8);
            SolidBrush drawBrush = new SolidBrush(Color.Black);

            //Angle Lines
            for (int i = 0; i < 19; ++i)
            {
                float cosValue = (float)Math.Cos(10.0 * i / 180 * Math.PI);
                float sinValue = (float)Math.Sin(10.0 * i / 180 * Math.PI);
                float x = (float)(centerX + width / 1.7 * cosValue);
                float y = (float)(centerY - width / 1.7 * sinValue);

                if ((i < 22) || (i > 32))
                {
                    g.DrawLine(Pens.Black, centerX, centerY, x, y);
                }

            }

            RectangleF drawRect = new RectangleF(centerX - 35, centerY, 50, 50);
            g.DrawString("0.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 58, centerY, 50, 50);
            g.DrawString("1.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 82, centerY, 50, 50);
            g.DrawString("1.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 105, centerY, 50, 50);
            g.DrawString("2.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 130, centerY, 50, 50);
            g.DrawString("2.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 153, centerY, 50, 50);
            g.DrawString("3.0", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 183, centerY, 50, 50);
            //g.DrawString("3.5", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 208, centerY, 50, 50);
            //g.DrawString("4.0", drawFont, drawBrush, drawRect);

            drawRect = new RectangleF(centerX + 15, centerY, 50, 50);
            g.DrawString("0.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 38, centerY, 50, 50);
            g.DrawString("1.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 63, centerY, 50, 50);
            g.DrawString("1.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 88, centerY, 50, 50);
            g.DrawString("2.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 113, centerY, 50, 50);
            g.DrawString("2.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 135, centerY, 50, 50);
            g.DrawString("3.0", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 165, centerY, 50, 50);
            //g.DrawString("3.5", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 190, centerY, 50, 50);
            //g.DrawString("4.0", drawFont, drawBrush, drawRect);


            drawRect = new RectangleF(centerX - 6, centerY - 162, 50, 50);
            g.DrawString("0", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 27, centerY - 210, 50, 50);
            //g.DrawString("-10", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 60, centerY - 202, 50, 50);
            //g.DrawString("-20", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 130, centerY - 85, 50, 50);
            g.DrawString("60", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 125, centerY - 165, 50, 50);
            //g.DrawString("-40", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 153, centerY - 140, 50, 50);
            //g.DrawString("-50", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 72, centerY - 142, 50, 50);
            g.DrawString("30", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 190, centerY - 78, 50, 50);
            //g.DrawString("-70", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 200, centerY - 43, 50, 50);
            //g.DrawString("-80", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 203, centerY - 8, 50, 50);
            //g.DrawString("-90", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 198, centerY + 28, 50, 50);

            //drawRect = new RectangleF(centerX - 42, centerY - 210, 50, 50);
            //g.DrawString("10", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 75, centerY - 202, 50, 50);
            //g.DrawString("20", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 90, centerY - 140, 50, 50);
            g.DrawString("30", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 140, centerY - 165, 50, 50);
            //g.DrawString("40", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 165, centerY - 140, 50, 50);
            //g.DrawString("50", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 147, centerY - 81, 50, 50);
            g.DrawString("60", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 203, centerY - 78, 50, 50);
            //g.DrawString("70", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 213, centerY - 43, 50, 50);
            //g.DrawString("80", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 215, centerY - 8, 50, 50);
            //g.DrawString("90", drawFont, drawBrush, drawRect);

            sLaserTopView.BackgroundImage = bmp;
            sLaserTopView2.BackgroundImage = bmp;
            sLaserTopView3.BackgroundImage = bmp;
        }
        private void sDrawSensor(int[] data, int robotID)
        {
            //DRAW THE LASER FROM TOP VIEW
            Bitmap sensorImge = new Bitmap(sLaserTopView.Width, sLaserTopView.Height);
            Graphics g = Graphics.FromImage(sensorImge);
            int width = sensorImge.Width - WIDTH_SUB;
            int height = sensorImge.Height;

            int centerX = sensorImge.Width / 2 - START_X, centerY = sensorImge.Height - START_Y + 85;

            g.Clear(Color.Transparent);

            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;

            SolidBrush dataBrush = null;
            PointF[] curvePoints = new PointF[data.Length + 1];

            // Define fill mode.
            FillMode newFillMode = FillMode.Winding;

            curvePoints[data.Length] = new PointF((float)centerX, (float)centerY);

            dataBrush = new SolidBrush(Color.Lime);
            for (int i = 0; i < data.Length; ++i)
            {
                //HERE WE MUST FIND THE INDEXES FOR i CORRESPONDING TO AN ANGLE RANGE FROM -65,65 AT LEAST
                if (data[i] > 3000)
                    data[i] = 3000;
                if (data[i] == 0)
                    data[i] = 20;
                float Y = (float)(centerY - ratio * (double)data[i] / 1000.0 * Math.Sin(0.5 * i * Math.PI / 180.0));
                float X = (float)(centerX + ratio * (double)data[i] / 1000.0 * Math.Cos(0.5 * i * Math.PI / 180.0));
                curvePoints[i] = new PointF(X, Y);
            }
            // Fill polygon to screen.
            g.FillPolygon(dataBrush, curvePoints, newFillMode);
            
            if (robotID ==1)
                sLaserTopView.Image = sensorImge;
            else if (robotID ==2)
                sLaserTopView2.Image = sensorImge;
            else if (robotID ==3)
                sLaserTopView3.Image = sensorImge;


            //DRAW THE LASER FROM FRONT VIEW

            Bitmap bmp = new Bitmap(sLaserFrontView.Width, sLaserFrontView.Height);
            Graphics gF = Graphics.FromImage(bmp);
            int widthF = bmp.Width;

            gF.Clear(Color.Lavender);
            int half = bmp.Height / 2;

            for (int x = 0; x < data.Length; x++)
            {
                if (data[x] > 3000)
                    data[x] = 3000;
                if (data[x] == 0)
                    data[x] = 20;
                Color col = LinearColor(Color.Red, Color.Lavender, 0, 3500, data[x]);
                gF.DrawLine(new Pen(col), widthF - x, half - (half / 12) * 3000 / data[x], widthF - x, half + (half / 12) * 3000 / data[x]);
            }

            
            if (robotID == 1)
                sLaserFrontView.Image = bmp;
            else if (robotID == 2)
                sLaserFrontView2.Image = bmp;
            else if (robotID == 3)
                sLaserFrontView3.Image = bmp;

        }

        // Local copy of the image
        private Bitmap _MapImage;
        public Bitmap MapImage
        {
            get { return _MapImage; }
            set
            {
                _MapImage = value;

                Image old = picMap.Image;
                picMap.Image = value;

            }
        }
        private Bitmap _MapImage2;
        public Bitmap MapImage2
        {
            get { return _MapImage2; }
            set
            {
                _MapImage2 = value;

                Image old = picMap2.Image;
                picMap2.Image = value;

            }
        }
        private Bitmap _MapImage3;
        public Bitmap MapImage3
        {
            get { return _MapImage3; }
            set
            {
                _MapImage3 = value;

                Image old = picMap3.Image;
                picMap3.Image = value;

            }
        }
        #endregion

        //Control Buttons for every robot
        private void sbtnSetManual_Click(object sender, EventArgs e)
        {
            manualcontrol = !manualcontrol;
            if (manualcontrol == true)
            {      
                sbtnSetManual.Text = "STOP MANUAL CONTROL";
                ManualControls1.Enabled = true;
            }
            else
            {
                ManualControls1.Enabled = false;
                sbtnSetManual.Text = "SET TO MANUAL CONTROL";
                sLeftWheelsSpeedTrackBar.Value = 0;
                sRightWheelsSpeedTrackBar.Value = 0;
                sDrivingSpeedTrackBar.Value = 0;
                sSteeringSpeedTrackBar.Value = 0;
            }
        }
        private void sEmergencyStop_Click(object sender, EventArgs e)
        {
            sLeftWheelsSpeedTrackBar.Value = 0;
            sRightWheelsSpeedTrackBar.Value = 0;
            sDrivingSpeedTrackBar.Value = 0;
            sSteeringSpeedTrackBar.Value = 0;
        }
        private void sbtnReleaseControl_Click(object sender, EventArgs e)
        {
            releasecontrol = !releasecontrol;
            if (releasecontrol == true)
                sbtnReleaseControl.Text = "HOLD CONTROL";
            else
            {
                sbtnReleaseControl.Text = "RELEASE ROBOT CONTROL";
                sLeftWheelsSpeedTrackBar.Value = 0;
                sRightWheelsSpeedTrackBar.Value = 0;
                sDrivingSpeedTrackBar.Value = 0;
                sSteeringSpeedTrackBar.Value = 0;
            }
        }
        private void sbtnRecord_Click(object sender, EventArgs e)
        {

        }

        private void sbtnSetManual2_Click(object sender, EventArgs e)
        {
            manualcontrol2 = !manualcontrol2;
            if (manualcontrol2 == true)
            {
                sbtnSetManual2.Text = "STOP MANUAL CONTROL";
                ManualControls2.Enabled = true;
            }
            else
            {
                ManualControls2.Enabled = false;
                sbtnSetManual2.Text = "SET TO MANUAL CONTROL";
                sLeftWheelsSpeedTrackBar2.Value = 0;
                sRightWheelsSpeedTrackBar2.Value = 0;
                sDrivingSpeedTrackBar2.Value = 0;
                sSteeringSpeedTrackBar2.Value = 0;
            }
        }
        private void sEmergencyStop2_Click(object sender, EventArgs e)
        {
            sLeftWheelsSpeedTrackBar2.Value = 0;
            sRightWheelsSpeedTrackBar2.Value = 0;
            sDrivingSpeedTrackBar2.Value = 0;
            sSteeringSpeedTrackBar2.Value = 0;
        }
        private void sbtnReleaseControl2_Click(object sender, EventArgs e)
        {
            releasecontrol2 = !releasecontrol2;
            if (releasecontrol2 == true)
                sbtnReleaseControl2.Text = "HOLD CONTROL";
            else
            {
                sbtnReleaseControl2.Text = "RELEASE ROBOT CONTROL";
                sLeftWheelsSpeedTrackBar2.Value = 0;
                sRightWheelsSpeedTrackBar2.Value = 0;
                sDrivingSpeedTrackBar2.Value = 0;
                sSteeringSpeedTrackBar2.Value = 0;
            }
        }
        private void sbtnRecord2_Click(object sender, EventArgs e)
        {

        }

        private void sbtnSetManual3_Click(object sender, EventArgs e)
        {
            manualcontrol3 = !manualcontrol3;
            if (manualcontrol3 == true)
            {
                sbtnSetManual3.Text = "STOP MANUAL CONTROL";
                ManualControls3.Enabled = true;
            }
            else
            {
                ManualControls3.Enabled = false;
                sbtnSetManual3.Text = "SET TO MANUAL CONTROL";
                sLeftWheelsSpeedTrackBar3.Value = 0;
                sRightWheelsSpeedTrackBar3.Value = 0;
                sDrivingSpeedTrackBar3.Value = 0;
                sSteeringSpeedTrackBar3.Value = 0;
            }
        }
        private void sEmergencyStop3_Click(object sender, EventArgs e)
        {
            sLeftWheelsSpeedTrackBar3.Value = 0;
            sRightWheelsSpeedTrackBar3.Value = 0;
            sDrivingSpeedTrackBar3.Value = 0;
            sSteeringSpeedTrackBar3.Value = 0;
        }
        private void sbtnReleaseControl3_Click(object sender, EventArgs e)
        {
            releasecontrol3 = !releasecontrol3;
            if (releasecontrol3 == true)
                sbtnReleaseControl3.Text = "HOLD CONTROL";
            else
            {
                sbtnReleaseControl3.Text = "RELEASE ROBOT CONTROL";
                sLeftWheelsSpeedTrackBar3.Value = 0;
                sRightWheelsSpeedTrackBar3.Value = 0;
                sDrivingSpeedTrackBar3.Value = 0;
                sSteeringSpeedTrackBar3.Value = 0;
            }
        }
        private void sbtnRecord3_Click(object sender, EventArgs e)
        {

        }




    }
}
